﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace ODESolver
{
    public static partial class OdeSolver
    {

        static double[] cf = { 0.0, 1.0 / 4.0, 3.0 / 8.0, 12.0 / 13.0, 1.0, 1.0 / 2.0 };
        static int[] hlpf = { 0, 1, 2, 3, 4, 5};
        static double[,] af = { 
                           //  { 0, 0, 0, 0, 0, 0, 0 }, 
                             { 1.0 / 4.0, 0, 0, 0, 0, 0 },
                             { 3.0 / 32.0, 9.0 / 32.0, 0, 0, 0, 0}, 
                             { 1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0, 0, 0, 0}, 
                             { 439.0 / 216.0, -8.0, 3680.0 / 513.0, -845.0 / 4104.0, 0, 0 }, 
                             { -8.0 / 27.0, 2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0, 0}, 
                             { -41.0 / 260.0, 22.0 / 13.0, 43.0 / 156.0, -118.0 / 39.0, 32.0 / 195.0, 80.0 / 39.0} };

        static double[] bf = { 25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0 };
        static double[] df = { 16.0 / 135.0, 0.0, 6656.0 / 12825.0, 28561.0 / 56430.0, -9.0 / 50.0, 2.0 / 55.0 };


        /// <summary>
        /// Integrate on time T by Fehlberg
        /// </summary>
        /// <param name="f">System of Ordinary Diff Equations</param>
        /// <param name="T">end point of integration</param>
        /// <param name="Tstep">time step</param>
        /// <param name="init">vector of initial state</param>
        /// <param name="arg">parametrs of ODESys</param>
        /// <returns></returns>
        static private double[] FehlbergOnTime(ODESys f, double T, double Tstep, double[] init, double[] arg)
        {
            //throw new NotImplementedException();

            int N = init.Length;
            double[] Y = new double[N];
            double t = 0;//current t
            init.CopyTo(Y, 0);
            double orig = Tstep;
            while (t <= T)
            {
                Y = FehlbergOneStep(f, t, Tstep, Y, arg);
                t += Tstep;
                Tstep = orig;
            }

            return Y;
        }

        static private double[,] FehlbergTrajectory(ODESys f, double T, double Tstep, double[] init, double[] arg)
        {
            //throw new NotImplementedException();
            int N = init.Length;
            double tmp = T / Tstep;
            int Nsteps = (int)Math.Truncate(tmp);
            double[,] Y = new double[Nsteps, N];
            double[] Ytemp = new double[N];
            double t = 0;
            init.CopyTo(Ytemp, 0);


            for (int i = 0; i < Nsteps; i++)
            {
                for (int j = 0; j < N; j++)
                    Y[i, j] = Ytemp[j];

                Ytemp = ButcherOneStep(f, t, Tstep, Ytemp, arg);
                t += Tstep;
            }

            return Y;
        }



        static private double[] FehlbergOneStep(ODESys f, double t, double dT, double[] vector, double[] arg)
        {
            double err = 1.0;
            int N = vector.Length;
            double[] y = new double[N];
            int[] hlp = new int[N];
            for (int i = 0; i < hlp.Length; i++)
                hlp[i] = i;
            double[] u = new double[N];
            double[][] k = new double[6][];


            vector.CopyTo(y, 0);
            vector.CopyTo(u, 0);

            for (int n = 0; n < 5; n++)
            {
                k[n] = (from i in hlp
                        select f(i, t + cf[n] * dT, u, arg)).ToArray();

                for (int i = 0; i < N; i++)
                {
                    double tmp = (from m in hlpf
                                  select k[n][i] * af[n, m]).Sum();
                    u[i] = y[i] + dT * tmp;
                }
            }
            k[5] = (from i in hlp
                    select f(i, t + cf[5] * dT, u, arg)).ToArray();
            for (int j = 0; j < N; j++)
            {
                y[j] = y[j] + dT * (from i in hlpf
                                    select k[i][j] * df[i]).Sum();
                err += Math.Abs((from i in hlpf
                                 select k[i][j] * (bf[i] - df[i])).Sum());
            }
            err *= dT;

            //if (dT > 1e-15)
            //    step = dT;
            return y;
        }

        
    }
}
